//
// Created by 12345 on 2025/2/19.
//

#include "command_bean.h"


std::string CommandBean::build() {
    sendPayload = "";

    uint8_t payloadBuffer[56] = {0x00};
    //固定头,尾部
    payloadBuffer[0] = 0x5A;
    payloadBuffer[1] = 0xA4;
    payloadBuffer[2] = 0x38;
    payloadBuffer[3] = 0xA5;
    payloadBuffer[4] = 0x03;
    payloadBuffer[5] = 0x01;
    payloadBuffer[6] = 0x01;
    payloadBuffer[7] = 0x01;
    payloadBuffer[55] = 0xff;

    //如果站起坐下,第9字节第4位为1: 0001 0000
    uint8_t byte9 = 0x00;
    if (standSitFlag) {
        byte9 = byte9 | 0x10;//0001 0000
    } else {
        byte9 = byte9 & 0xef;//1110 1111
    }
    //如果解锁力控,第9字节第8位为1:
    if (unlockFlag) {
        byte9 = byte9 | 0x01;//0000 0001
    } else {
        byte9 = byte9 & 0xfe;//1111 1110
    }
    payloadBuffer[8] = byte9;
    if (scramFlag) {
        payloadBuffer[9] = 0x01;
    }
    payloadBuffer[10] = modeCode;
    payloadBuffer[11] = static_cast<uint8_t>((forwardBackDir << 7) | forwardBackSpeed);;
    payloadBuffer[12] = static_cast<uint8_t>((leftRightDir << 7) | leftRightSpeed);
    payloadBuffer[13] = static_cast<uint8_t>((leftRightTurnDir << 7) | leftRightTurnSpeed);
    payloadBuffer[14] = gaitCode;
    payloadBuffer[15] = static_cast<uint8_t>((fuYangDir << 7) | fuYangSpeed);
    payloadBuffer[18] = chargeCode;
    //第27字节第一位代表目标识别，第二位代表目标跟踪
    payloadBuffer[27] = targeShibieCode;
    if (targeGenzongCode == 1) {
        payloadBuffer[27] = (payloadBuffer[27] | 0x02);
    } else {
        payloadBuffer[27] = (payloadBuffer[27] & 0xfd); //0xfd 1111 1101
    }

    if (video1OptionCode == 1) {
        payloadBuffer[25] = 0x01;
        payloadBuffer[34] = 0x05;
        payloadBuffer[35] = 0x05;
        payloadBuffer[36] = 0x00;
    } else if (video1OptionCode == 2) {
        payloadBuffer[25] = 0x02;
        payloadBuffer[34] = 0x05;
        payloadBuffer[35] = 0x05;
        payloadBuffer[36] = 0x01;
    } else {
        payloadBuffer[25] = 0x01;
        payloadBuffer[34] = 0x00;
        payloadBuffer[35] = 0x00;
        payloadBuffer[36] = 0x00;
    }

    if (zaiheType != 0 && zaiheCmd != 0) {
        payloadBuffer[34] = zaiheType;
        payloadBuffer[35] = zaiheCmd;
        payloadBuffer[36] = zaiheData;
    }
    payloadBuffer[41] = 0x95;
    payloadBuffer[42] = (daji << 2) | baoxian;
    payloadBuffer[43] = (mubiaoqueren << 2) | mubiaoxuanze;
    payloadBuffer[45] = huokongjiesuan;

    //校验位
    uint8_t cka = 0x00;
    uint8_t ckb = 0x00;
    int checkSum = 0;
    for (int i = 0; i < 53; i++) {
        checkSum = checkSum + payloadBuffer[i];
    }
    checkSum = checkSum + 0xff;
    cka = (checkSum & 0x000000ff);
    ckb = (checkSum & 0x0000ff00) >> 8;
    payloadBuffer[53] = cka;
    payloadBuffer[54] = ckb;
    sendPayload.assign(reinterpret_cast<const char *>(payloadBuffer), 56);

    return sendPayload;
}

void CommandBean::reset() {
    standSitFlag = false;
    unlockFlag = false;
    modeCode = 0;
    scramFlag = false;
    chargeCode = 0;
    zaiheType = 0;
    zaiheCmd = 0;
    zaiheData = 0;
    daji = 0;
    mubiaoxuanze = 0;
    mubiaoqueren = 0;
}

CommandBean *CommandBean::getInstance() {
    static CommandBean instance;
    return &instance;
}


void
CommandBean::forceCode(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack) {
    unlockFlag = true;
}

void CommandBean::standSitCode(const DeviceAgentModel::DeviceAgentProperty &cmd,
                               DeviceAgentModel::DeviceAgentProperty &ack) {
    standSitFlag = true;
}

void
CommandBean::scramCode(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack) {
    scramFlag = true;
}

void
CommandBean::tabu(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack) {
    //如果上一次是RL模式,那么先停止RL
    if (ReportBean::getInstance().modeCode == 2) {
        modeCode = 3;
        mp_log_info(DEFAULT_LOGGER, "command_bean pre mode is rl,stop rl");
    }
    modeCode = 1;
    ReportBean::getInstance().modeCode = modeCode;
}

void
CommandBean::markTime(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack) {

    //进入踏步模式，设置对应踏步状态
    gaitCode = static_cast<uint8_t>(cmd.property_int().value());
    mp_log_info(DEFAULT_LOGGER, "command_bean markTime enter gaitCode:%d", gaitCode);
}


void CommandBean::reinforcementLearningCode(const DeviceAgentModel::DeviceAgentProperty &cmd,
                                            DeviceAgentModel::DeviceAgentProperty &ack) {
    modeCode = 2;
    ReportBean::getInstance().modeCode = modeCode;
    mp_log_info(DEFAULT_LOGGER, "command_bean rl enter");
}

void
CommandBean::modeStopCode(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack) {

    //模式取消
    modeCode = 3;
    mp_log_info(DEFAULT_LOGGER, "command_bean modeStopCode");
}


void CommandBean::ctlJoy(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack) {
    //左摇杆控制前后左右   右摇杆控制转向俯仰
    auto x = cmd.property_struct().members().find("x")->second.property_int().value();
    if (x > 0) {
        leftRightDir = 1;
    } else {
        leftRightDir = 0;
    }
    leftRightSpeed = scaleJoyValue(std::abs(x), 0, 100);
    if(leftRightSpeed<20){
        leftRightSpeed=0;
    }
    auto y = cmd.property_struct().members().find("y")->second.property_int().value();
    if (y > 0) {
        forwardBackDir = 0;
    } else {
        forwardBackDir = 1;
    }
    forwardBackSpeed = scaleJoyValue(std::abs(y), 0, 100);
    if(forwardBackSpeed<20){
        forwardBackSpeed=0;
    }
    auto x1 = cmd.property_struct().members().find("x1")->second.property_int().value();
    if (x1 > 0) {
        leftRightTurnDir = 1;
    } else {
        leftRightTurnDir = 0;
    }
    leftRightTurnSpeed = scaleJoyValue(std::abs(x1), 0, 100);
    auto y1 = cmd.property_struct().members().find("y1")->second.property_int().value();
    if (y1 > 0) {
        fuYangDir = 1;
    } else {
        fuYangDir = 0;
    }
    fuYangSpeed = scaleJoyValue(std::abs(y1), 0, 100);

    mp_log_info(DEFAULT_LOGGER, "command_bean ctlJoy x:%d y:%d x1:%d y1:%d", x, y, x1, y1);
    mp_log_info(DEFAULT_LOGGER,
                "command_bean ctlJoy leftRightDir:%d leftRightSpeed:%d forwardBackDir:%d forwardBackSpeed:%d leftRightTurnDir:%d leftRightTurnSpeed:%d fuYangDir:%d fuYangSpeed:%d",
                leftRightDir, leftRightSpeed, forwardBackDir, forwardBackSpeed, leftRightTurnDir, leftRightTurnSpeed,
                fuYangDir, fuYangSpeed);


}

void CommandBean::chargeControl(const DeviceAgentModel::DeviceAgentProperty &cmd,
                                DeviceAgentModel::DeviceAgentProperty &ack) {
    chargeCode = static_cast<uint8_t>(cmd.property_int().value());
    mp_log_info(DEFAULT_LOGGER, "command_bean chargeControl %d", chargeCode);
}

void
CommandBean::videoStream(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack) {
    video1OptionCode = static_cast<uint8_t>(cmd.property_int().value());
    mp_log_info(DEFAULT_LOGGER, "command_bean videoStream %d", video1OptionCode);
}

void CommandBean::targetIdentity(const DeviceAgentModel::DeviceAgentProperty &cmd,
                                 DeviceAgentModel::DeviceAgentProperty &ack) {

    targeShibieCode = static_cast<uint8_t>(cmd.property_int().value());
    mp_log_info(DEFAULT_LOGGER, "command_bean targetIdentity %d", targeShibieCode);
}

void CommandBean::targetTrackin(const DeviceAgentModel::DeviceAgentProperty &cmd,
                                DeviceAgentModel::DeviceAgentProperty &ack) {
    targeGenzongCode = static_cast<uint8_t>(cmd.property_int().value());
    mp_log_info(DEFAULT_LOGGER, "command_bean targeGenzongCode %d", targeGenzongCode);

}

void CommandBean::targetSelection(const DeviceAgentModel::DeviceAgentProperty &cmd,
                                  DeviceAgentModel::DeviceAgentProperty &ack) {
    auto centerX = cmd.property_struct().members().at("centerX").property_int().value();
    auto centerY = cmd.property_struct().members().at("centerY").property_int().value();

}

void
CommandBean::assurance(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack) {
    baoxian = static_cast<uint8_t>(cmd.property_int().value());
    mp_log_info(DEFAULT_LOGGER, "command_bean baoxian %d", baoxian);
}

void CommandBean::attack(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack) {
    daji = static_cast<uint8_t>(cmd.property_int().value());
    mp_log_info(DEFAULT_LOGGER, "command_bean daji %d", daji);
}

void CommandBean::attackChoice(const DeviceAgentModel::DeviceAgentProperty &cmd,
                               DeviceAgentModel::DeviceAgentProperty &ack) {
    mubiaoxuanze = static_cast<uint8_t>(cmd.property_int().value());
    mp_log_info(DEFAULT_LOGGER, "command_bean mubiaoxuanze %d", mubiaoxuanze);
}

void
CommandBean::objidentify(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack) {
    mubiaoqueren = static_cast<uint8_t>(cmd.property_int().value());
    mp_log_info(DEFAULT_LOGGER, "command_bean mubiaoqueren %d", mubiaoqueren);
}

void
CommandBean::calculate(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack) {
    huokongjiesuan = static_cast<uint8_t>(cmd.property_int().value());
    mp_log_info(DEFAULT_LOGGER, "command_bean huokongjiesuan %d", huokongjiesuan);
}





